#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy, sys
from time import sleep
from sg805_driver.msg import ExtraFeatures


class ExtraFeaturesDemo():
    def __init__(self):
        rospy.init_node('extraFeaturesCommander', anonymous = False )
        self.pub = rospy.Publisher("/sg805/extraFeatures", ExtraFeatures, queue_size=10)
        self.extraMsg = ExtraFeatures()

        
        #根据功能编号进行调用，参考 sg805RobotRos中 extraFeaturesCB回调函数的内容。
        # extraMsg.Tag = 8
        # PWM_START = 6,    // 打开PWM
        # PWM_STOP = 7,     // 关闭PWM

        print("等待话题连接 2秒")
        sleep(2)

        self.grasp()
        #self.release()

        # while (not rospy.is_shutdown()):  
        #     self.grasp()
        #     sleep(5)
        #     self.release()
        #     sleep(5)
            


    # 夹取
    def release(self):
        self.extraMsg.Tag = 6
        self.extraMsg.PSC = 19999
        self.extraMsg.ARR = 9
        self.extraMsg.CCR1 = 5
        self.extraMsg.PluseCount = 6400 * 22
        self.pub.publish(self.extraMsg)


    # 释放
    def grasp(self):
        self.extraMsg.Tag = 6
        self.extraMsg.PSC = 19999
        self.extraMsg.ARR = 9
        self.extraMsg.CCR1 = 5
        self.extraMsg.PluseCount = -6400 * 22
        self.pub.publish(self.extraMsg)


if __name__ == '__main__':
    try:
        ExtraFeaturesDemo()
    except rospy.ROSInterruptException:
        pass